#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float64MultiArray.h"
#include <linux/input.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <iostream>
#include <vector>
#include <fstream>
#include <stdio.h>
#include <stdlib.h>
#include "key_board.h"
#include "std_msgs/Int16MultiArray.h"
#include "didi_msgs/Pose2DStamped.h"

using namespace std;

namespace key_board {


   key_board::key_board():
   event{0},
   key_fd{0}
   //key_board_event{"/dev/input/event10"}
   {
       printf("build key_board_node success \r\n");
       Control_Command = nh.advertise<std_msgs::Int16MultiArray>("key_board/topic_command",10);
   }

   key_board::~key_board(){

       printf("destroy key_board_node success \r\n");
   }

   void  key_board::send_control_command(int16_t speed_enable,int16_t target_speed,int16_t dir_enable,int16_t target_angle,int16_t  target_dir){


        //printf("hello1111111\r\n") ;

        std_msgs::Int16MultiArray con_com;
        con_com.data.push_back(speed_enable);
        con_com.data.push_back(target_speed);
        con_com.data.push_back(dir_enable);
        con_com.data.push_back(target_angle);
        con_com.data.push_back(target_dir);
        Control_Command.publish(con_com);

   }
   void  key_board::key_board_run(){


      ros::Rate loop_rate(100);//一个ros::Rate对象允许user制定循环的频率

      key_fd = open("/dev/input/event2", O_RDONLY|O_NONBLOCK);
      printf("/dev/input/event10 open&&&&&&&&&&&&&&&&&&&&&&&&&& \n\r");
      printf("key_fd:%d\r\n",key_fd);
      if(key_fd <= 0){
          printf("open device error\n");
          return ;
      }

      while (nh.ok()){

         read_key_board();
         loop_rate.sleep();
      }
   }

   void key_board::read_key_board()
   {

     //struct input_event event;
     if(read(key_fd, &event, sizeof(event)) == sizeof(event)){

        if (event.type == EV_KEY){

            if (event.value == 0 || event.value == 1){

                printf("key %d %s\n", event.code, (event.value) ? "Pressed" : "Released");

                if(event.code == KEY_ESC) {



                }else if(event.code == 103 && event.value==1){ //

                        send_control_command(1,1010,0,400,0);
                        usleep(30000);
                        printf("go ahead!!!!!!!!!\n\r");

                }else if(event.code == 108 && event.value==1){

                        usleep(30000);
                        send_control_command(1,2000,0,400,0);
                        printf("back################\n\r");

                }else if(event.code == 105 && event.value==1){

                        send_control_command(0,1000,1,400,0);
                        usleep(30000);
                        printf("turn left%%%%%%%%%%%%%%%%%%\n\r");

                }else if(event.code == 106 && event.value==1) {

                        send_control_command(0,1000,1,400,1);
                        usleep(30000);
                        printf("turn right%%%%%%%%%%%%%%%%%%\n\r");

                }else if(event.value==0){

                        if((event.code == 106)||(event.code == 105)){

                        }else {

                            usleep(30000);
                            printf("stop@@@@@@@@@@@code=%d\n\r",event.code);
                        }
                 }

            }
          }

       }

   }

}
